#!/usr/bin/env python
import rospy
from std_msgs.msg import Bool
from geometry_msgs.msg import Pose
from moveit_commander import MoveGroupCommander
import moveit_commander
import sys
import numpy as np
from tf.transformations import quaternion_matrix
from geometry_msgs.msg import PoseStamped, TransformStamped
import tf2_ros
import time
import copy

right_pose = None
def right_pose_callback(msg):
    global right_pose
    right_pose = msg

# 用于接收 adjusted_pose_left 和 start_move 信号
left_pose = None
last_valid_pose = None
task = "press"
start_move = False  # 初始时，start_move 设为 False

def left_pose_callback(msg):
    global left_pose, last_valid_pose, task, start_move

    if not start_move:  # 如果 start_move 为 False，则不执行动作
        rospy.loginfo("Waiting for start_move signal to be True.")
        return

    # 检查当前 pose 是否有效（避免接收到无效位姿）
    if msg.pose.position.x == -1 or msg.pose.position.y == -1 or msg.pose.position.z == -1:
        if last_valid_pose:
            rospy.loginfo("Invalid pose received, using last valid pose.")
            left_pose = copy.deepcopy(last_valid_pose)
        else:
            rospy.logwarn("No valid pose available, skipping this callback.")
            return
    else:
        # 计算当前位置与上次有效位置之间的距离
        if last_valid_pose:
            current_position = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
            last_position = np.array([last_valid_pose.pose.position.x, last_valid_pose.pose.position.y, last_valid_pose.pose.position.z])
            distance = np.linalg.norm(current_position - last_position)

            # 如果距离大于 1cm，视为无效位姿，不进行规划
            if distance > 0.03:
                rospy.loginfo(f"Position change exceeds 1cm ({distance:.4f}m), skipping this pose.")
                return
            else:
                left_pose = copy.deepcopy(msg)
                last_valid_pose = copy.deepcopy(msg)  # 保存当前有效的 pose
        else:
            left_pose = copy.deepcopy(msg)
            last_valid_pose = copy.deepcopy(msg)  # 保存当前有效的 pose

    rospy.loginfo("Received a pose, starting movement.")

    quaternion = [left_pose.pose.orientation.x, left_pose.pose.orientation.y, left_pose.pose.orientation.z, left_pose.pose.orientation.w]
    rotation_matrix = quaternion_matrix(quaternion)

    local_offset1 = np.array([0.0, 0.0, 0.00, 1.0])  # [世界坐标系y轴（），世界坐标系z轴（数据增加z轴值减小，向物体下方移动），世界坐标系x轴（数据增加x轴值减少，离物体越远）]
    local_offset2 = np.array([0.0, 0.0, 0.05, 1.0])  # [世界坐标系y轴（），世界坐标系z轴（数据增加z轴值减小，向物体下方移动），世界坐标系x轴（数据增加x轴值减少，离物体越远）]
    world_offset1 = np.dot(rotation_matrix, local_offset1)
    world_offset2 = np.dot(rotation_matrix, local_offset2)

    rate = rospy.Rate(1)
    if task == "press":
        print(task)
        msg_hand_control1 = Bool()
        msg_hand_control1.data = True

        left_pose.pose.position.x += 0.04
        left_pose.pose.position.y += -0.09
        left_pose.pose.position.z += 0.15
        '''
        
        left_pose.pose.orientation.x = 0.11236370921869202
        left_pose.pose.orientation.y = 0.15079563957693937
        left_pose.pose.orientation.z = 0.7905900885524749
        left_pose.pose.orientation.w = 0.5827541366801177
        
        left_pose.pose.orientation.x = -0.0321
        left_pose.pose.orientation.y = 0.1140
        left_pose.pose.orientation.z = 0.9236
        left_pose.pose.orientation.w = 0.3645
        '''

        left_pose.pose.orientation.x = -0.014
        left_pose.pose.orientation.y = 0.332
        left_pose.pose.orientation.z = 0.912
        left_pose.pose.orientation.w = 0.24

    

        move_to_pose(left_pose, "Right_Arm_Link7")
        start_move = False
        task = "release"

    elif task == "release":
        print(task)
        msg_hand_control1 = Bool()
        msg_hand_control1.data = True
        left_pose.pose.position.x += -0.06
        left_pose.pose.position.y += -0.05
        left_pose.pose.position.z += 0.15

        left_pose.pose.orientation.x = 0.11236370921869202
        left_pose.pose.orientation.y = 0.15079563957693937
        left_pose.pose.orientation.z = 0.7905900885524749
        left_pose.pose.orientation.w = 0.5827541366801177

        move_to_pose(left_pose, "Right_Arm_Link7")

        start_move = False
        task = "press"

def move_to_pose(pose, link_name):
    """
    移动手臂到指定位姿，使用分步规划策略。
    :param pose: Pose 消息，目标位姿
    :param link_name: 要移动的链接名称
    """
    global move_group
    print("adjusted_left_pose", pose)
    print(move_group.get_current_pose().pose)

    current_pose = move_group.get_current_pose().pose

    # 计算当前位置与目标位置之间的差距
    distance = np.linalg.norm(np.array([left_pose.pose.position.x, left_pose.pose.position.y, left_pose.pose.position.z]) -
                              np.array([current_pose.position.x, current_pose.position.y, current_pose.position.z]))
    print(f"Distance to target pose: {distance}")

    # 设定一个阈值，如果目标位姿与当前位置的距离大于这个值，则进行分步规划
    step_size = 0.50  # 每一步的距离，单位米
    if distance > step_size:
        num_steps = int(np.ceil(distance / step_size))  # 计算需要的步数

        # 按照分步的策略逐步移动
        for i in range(num_steps):
            # 计算每一步的目标位置
            interpolation_factor = (i + 1) / num_steps  # 计算每一步的插值因子
            intermediate_pose = Pose()
            intermediate_pose.position.x = current_pose.position.x + (left_pose.pose.position.x - current_pose.position.x) * interpolation_factor
            intermediate_pose.position.y = current_pose.position.y + (left_pose.pose.position.y - current_pose.position.y) * interpolation_factor
            intermediate_pose.position.z = current_pose.position.z + (left_pose.pose.position.z - current_pose.position.z) * interpolation_factor

            # 使用当前的位姿插值更新目标姿态
            intermediate_pose.orientation = left_pose.pose.orientation  # 保持目标的旋转信息不变

            move_group.set_pose_target(intermediate_pose)
            plan, traj, planning_time, error_code = move_group.plan()

            if plan:
                success = move_group.execute(traj, wait=True)
                if not success:
                    rospy.logwarn(f"Failed to move {link_name} to intermediate pose at step {i+1}.")
                    # 如果某一步执行失败，尝试调整目标位置
                    intermediate_pose.position.z += 0.01  # 稍微调整Z轴
                    move_group.set_pose_target(intermediate_pose)
                    plan, traj, planning_time, error_code = move_group.plan()
                    if plan:
                        success = move_group.execute(traj, wait=True)
                        if not success:
                            rospy.logwarn(f"Failed again after adjusting Z-axis, terminating.")
                            return
            else:
                rospy.logwarn(f"Failed to generate plan for {link_name} at step {i+1}.")
                return  # 如果某一步规划失败，则终止

        rospy.loginfo(f"Successfully moved {link_name} to the target pose in {num_steps} steps.")
    else:
        # 如果距离较小，直接规划到目标位姿
        move_group.set_pose_target(pose)
        plan, traj, planning_time, error_code = move_group.plan()

        if plan:
            success = move_group.execute(traj, wait=True)
            if success:
                rospy.loginfo(f"Successfully moved {link_name} to the target pose.")
            else:
                rospy.logwarn(f"Failed to move {link_name} to the target pose.")
        else:
            rospy.logwarn(f"Failed to generate plan for {link_name}.")

# 回调函数，用于接收 start_move 信号
def start_move_callback(msg):
    global start_move
    if msg.data:
        rospy.loginfo("Received start_move signal, proceeding with motion.")
        start_move = True
    else:
        rospy.loginfo("Received stop_move signal, halting motion.")
        start_move = False

# 主循环
if __name__ == "__main__":
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_arm_to_poses', anonymous=True)

    move_group = MoveGroupCommander("right_arm")
    move_group.set_end_effector_link("right_tool_link")
    move_group.set_planning_time(0.5)  # 增加规划时间
    move_group.set_num_planning_attempts(10)  # 增加尝试次数
    move_group.set_max_velocity_scaling_factor(0.3)
    move_group.set_max_acceleration_scaling_factor(0.3)
    move_group.set_goal_position_tolerance(0.01)  # 位置容差
    move_group.set_goal_orientation_tolerance(0.2)  # 姿态容差
    move_group.set_planner_id("RRTConnect")  # 使用RRTConnect算法

    print(move_group.get_planning_frame())

    hand_control_publisher = rospy.Publisher("/hand_control_topic", Bool, queue_size=1)
    rospy.sleep(2)

    rospy.Subscriber('/paper_pose', PoseStamped, left_pose_callback, queue_size=1)
    rospy.Subscriber('/start_move', Bool, start_move_callback, queue_size=1)  # 订阅新的布尔话题
    rospy.spin()